#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/cloud_viewer.h> //点云查看窗口头文件
#include "opencv2/highgui.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>

using namespace cv;
using namespace std;

// RGB colour visualisation example
boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    // 创建3D窗口并添加点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud");
    // 设置点云中每个点的显示大小为2像素
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
    // 添加一个1.0单位长度的3D坐标系(通常以米为单位)
    viewer->addCoordinateSystem(1.0);
    // 初始化相机的视角参数，使点云能正确显示在窗口中
    viewer->initCameraParameters();
    return (viewer);
}
int main()
{
    // 文件读取
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    std::string path = "../2.pcd";
    std::string imgPath = "../2.jpeg";
    // std::string path = "../1705910353800.pcd";
    // std::string imgPath = "../1705910353800.jpg";
    pcl::io::loadPCDFile(path, *point_cloud);
    // PointCloudRoar(point_cloud, point_cloud_ptr);

    Mat img = imread(imgPath);
    // 检查图像是否为RGB格式
    if (img.channels() != 3)
    {
        cout << "RGB pics needed." << endl;
        return 0;
    }
    // 相机内参 3X3
    const Mat K = (Mat_<double>(3, 3) << 1.9506592935364870e+03, 0.0, 1.9770478473401959e+03,
                   0.0, 1.9508117738745232e+03, 1.0786204201895550e+03,
                   0.0, 0.0, 1.);
    // // 相机内参 3X4（用于投影）
    const Mat camera_par = (Mat_<double>(3, 4) << 1.9506592935364870e+03, 0.0, 1.9770478473401959e+03, 0,
                            0.0, 1.9508117738745232e+03, 1.0786204201895550e+03, 0,
                            0.0, 0.0, 1.0, 0);
    // 相机内参 3X3
    // const Mat K = (Mat_<double>(3, 3) << 1.3172400446222302e+03, 0.0, 9.4234782808815817e+02,
    //                0.0, 1.3169852030481454e+03, 5.7041532459153677e+02,
    //                0.0, 0.0, 1.);
    // 相机内参 3X4
    // const Mat camera_par = (Mat_<double>(3, 4) << 1.3172400446222302e+03, 0.0, 9.4234782808815817e+02, 0,
    //                         0.0, 1.3169852030481454e+03, 5.7041532459153677e+02, 0,
    //                         0.0, 0.0, 1.0, 0);
    // 相机畸变参数[k1, k2, p1, p2, k3]
    const cv::Mat D = (cv::Mat_<double>(5, 1) << -0.050519061674533024, -0.007992982752507883, 0.00970045657644595, -0.004354775040194558, 0.0);
    // const cv::Mat D = (cv::Mat_<double>(5, 1) << -4.3150011826701307e-01, 2.1542475552129178e-01,
    //                    -2.0045043295496738e-03, 1.4770475046372033e-03,
    //                    -6.0047502709028498e-02);
    cv::Mat UndistortImage;
    // 去除畸变
    cv::undistort(img, UndistortImage, K, D, K);
    int rows = UndistortImage.rows;
    int cols = UndistortImage.cols;

    // 世界坐标到相机坐标变换矩阵(外参)
    Mat t_word_to_cam = (Mat_<double>(4, 4) << 2.4747462378258280e-02, -9.9955232303502073e-01, -1.6839925611563663e-02, -9.2541271346932907e-02,
                         -1.3087302341509554e-02, 1.6519577885364300e-02, -9.9977861656954914e-01, 2.4302538338292576e+00,
                         9.9960858363250360e-01, 2.4962312041639460e-02, -1.2672595327247342e-02, -5.0924142692133323e+00,
                         0., 0., 0., 1);
    // Mat t_word_to_cam = (Mat_<double>(4, 4) << 9.9999273764190155e-01, -6.9308707215508941e-06, 3.8111173451705954e-03, 4.5635547637939400e-01,
    //                      -3.8108166584693232e-03, 1.0873870747968539e-02, 9.9993361610226528e-01, 2.2001466751098611e-02,
    //                      -4.8372008040063058e-05, -9.9994087769573647e-01, 1.0873765365747643e-02, -9.8995469510555267e-02,
    //                      0., 0., 0., 1);
    // 点云坐标到相机坐标变换矩阵
    cv::Mat t_cam_to_word;
    // 吉利车不求逆
    // cv::invert(t_word_to_cam, t_cam_to_word);
    // 创建一个4x1的矩阵，64位浮点型
    Mat word_h = Mat(4, 1, CV_64FC1);
    // 创建一个3x1的矩阵，64位浮点型
    Mat p_result = Mat(3, 1, CV_64FC1);
    // 创建一个4x1的矩阵，64位浮点型
    Mat cam_h = Mat(4, 1, CV_64FC1);

    double p_u, p_v, p_w;      // pics_uv1;(u for cols, v for lines!!!)
    double c_x, c_y, c_z, c_i; // clouds_xyz、intensity;

    for (int nIndex = 0; nIndex < point_cloud->points.size(); nIndex++)
    {
        // 获取点云坐标
        c_x = point_cloud->points[nIndex].x;
        c_y = point_cloud->points[nIndex].y;
        c_z = point_cloud->points[nIndex].z;
        // 创建一个4x1的矩阵
        word_h = (Mat_<double>(4, 1) << c_x, c_y, c_z, 1);
        // 点云坐标到图像坐标
        p_result = camera_par * t_word_to_cam * word_h;
        // 吉利车不求逆
        // p_result = camera_par * t_cam_to_word * word_h;
        // 提取图像深度值
        p_w = p_result.at<double>(2, 0);
        // 计算像素坐标
        p_u = (int)((p_result.at<double>(0, 0)) / p_w);
        p_v = (int)((p_result.at<double>(1, 0)) / p_w);
        // 判断像素坐标是否在图像范围内
        if (p_u >= 0 && p_u < cols && p_v >= 0 && p_v < rows && p_w > 0)
        {
            /*cout << "--" << p_u << "--" << cols << endl;
            cout << "***" << p_v << "--" << rows << endl;*/
            // 获取点云的颜色值(OpenCV默认为BGR)
            float r = UndistortImage.at<Vec3b>(p_v, p_u)[2];
            float g = UndistortImage.at<Vec3b>(p_v, p_u)[1];
            float b = UndistortImage.at<Vec3b>(p_v, p_u)[0];
            point_cloud->points[nIndex].r = r;
            point_cloud->points[nIndex].g = g;
            point_cloud->points[nIndex].b = b;
        }
        else
        {
            // 超出图像范围的点设为白色
            point_cloud->points[nIndex].r = 255;
            point_cloud->points[nIndex].g = 255;
            point_cloud->points[nIndex].b = 255;
        }
    }
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    viewer = rgbVis(point_cloud);
    // 主循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }
}
